/**
 * IK Arm
 *
 * James Stone      http://www.manofstone.com
 *
 * Based on Reach 1. 
 * Based on code from Keith Peters (www.bit-101.com)
 * 
 * The arm follows the position of the mouse by
 * calculating the angles with atan2(). 
 */

import processing.serial.*;

// String portname = "/dev/tty.usbserial-A5002rKW"; // james arduino
  String portname = "/dev/tty.usbserial-A1001NQr"; // thomas' arduino, w/ feet
// String portname = "/dev/tty.SerialPort-1"; // for testing
Serial port;  // Create object from Serial class


float z = 100;
float x = 100;
float y = 100;
float x2 = 100;
float z2 = 100;
float y2 = 100;
float segLength = 50;
float resol = 1;

float zFakMaux = 150;  // for moving w/o mouse
float zCanvasLoc = 155;  // z loc of piece to cut
float xCanvasWidth = 100;  // width of canvas
float xFakMaux = 0;  // for moving w/o mouse - starts from 0, +/- width
float FakStart = 20;
float FakMax = 80;
float yFakMaux = FakStart;
int FakCount = 0;
int FakCountMax = 6;

void setup() {
  size(400, 400);
  smooth(); 
  strokeWeight(1.0);
  stroke(0, 100);
  PFont fontA = loadFont("Ziggurat-HTF-Black-32.vlw");
  textFont(fontA, 12);
  
  // init serial
  port = new Serial(this, portname, 9600);
}

// servo 0, rotation of base
String angle2Servo0(float a) { // a = angle in radians    
   int center = 2830; // centered rotation
   int c90 = 4520; // 90deg to the right, clockwise
   int cc90 = 1140; // 90deg to the left, counter clockwise, calculated
   int value = center;  // if below is not met, a = 0 exactly
   if (a > 0) { // clockwise + angles
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
   } else if (a < 0) { // cc - angles
     value = (int)(center - (center - cc90) / 90 * abs((degrees(a))));
   }
  return "s0" + nf(value, 4); 
}

// servo 1, elevation of 1st arm segment from base, shoulder
String angle2Servo1(float a) { // a = angle in radians    
   int center = 1280; // parallel with the ground plane
   int c90 = 3030; // perpendicular to the ground plane
   int value = center;  // if below is not met, a = 0 exactly
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
  return "s1" + nf(value, 4); 
}

// servo 2, elevation of 2nd arm segment from base, elbow
String angle2Servo2(float a) { // a = angle in radians    
   int center = 3210; // centered rotation
   int c90 = 4860; // 90deg to the right, clockwise
   int cc90 = 1560; // 90deg to the left, counter clockwise, calculated
   int value = center;  // if below is not met, a = 0 exactly
   if (a > 0) { // clockwise + angles
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
   } else if (a < 0) { // cc - angles
     value = (int)(center - (center - cc90) / 90 * abs((degrees(a))));
   }
  return "s2" + nf(value, 4); 
}


void draw() {
  background(200);
  float dx = xFakMaux - x; 
  float dz = zFakMaux - z; // z = 100, 200x200 sketch, change the coord. system
  float dy = yFakMaux - y;
  float angle1 = atan2(dy, dz);   // calculates in radians pi to - pi
  
  float tz = zFakMaux - cos(angle1) * segLength;
  float ty = yFakMaux - sin(angle1) * segLength;
  dz = tz - z2;  // z2 is the center of the matrix
  dy = ty - y2;
  float angle2 = atan2(dy, dz);  
  z = z2 + cos(angle2) * segLength;
  y = y2 + sin(angle2) * segLength;  
  
   // angle1 = elbow angle, angle 2 = shoulder angle, angle3 = rotation of base
   
   // move servo 1, 1st arm seg
   // port.write(angle2Servo1(angle2));
   // move servo 2, 2nd arm seg
   // port.write(angle2Servo2(angle1));
   // move servo 0
   // port.write(angle2Servo0(angle3));


  yFakMaux+= resol; // more resolution
  // alternate direction
/*  if (FakCount % 2 == 1) {
    xFakMaux-= resol; // for smoother response
  } else {
    xFakMaux+= resol; // for smoother response
  }

*/
  
  if (yFakMaux >= FakMax) {
      yFakMaux = FakStart;
      xFakMaux = 0;
      line(155, 100, 155, 0); // draw line for comparison    
      delay(100);  // commented out cut in, out
  } /*else if (yFakMaux <= 40 && yFakMaux > 30) {  // knife in
        zFakMaux+=resol; 
        // xFakMaux++;
      } else if (yFakMaux >= 60 && yFakMaux < 70) {  // knife out
        zFakMaux-=resol;
        // xFakMaux--;
      } */
  line(zCanvasLoc, 100, zCanvasLoc, 0); // draw line for comparison
//  line(xFakMaux, 100, xFakMaux, 0); // draw line for comparison

  // handle arc and of arm for x movement across the canvas, from left to right
  fill(255, 0, 0);
  stroke(255, 0, 0);
  int xOffset = 300;
  int yOffset = 100;
  line(xOffset - xCanvasWidth/2, yOffset + zCanvasLoc, xOffset + xCanvasWidth/2 , yOffset + zCanvasLoc);  //canvas for ref
 // line(xOffset, yOffset, xOffset + xFakMaux, yOffset + zCanvasLoc); //from origin to ref point
  
  // float angle3 = atan2(xFakMaux, zCanvasLoc);  
  float angle3 = -atan2(xFakMaux, zCanvasLoc); //+ radians(90);  
  x = cos(angle2) * zCanvasLoc;
  float yCan = sin(angle2) * zCanvasLoc;
  // for ref
   stroke(1, 255, 0);
  line(xOffset, yOffset, xOffset + xFakMaux, yOffset + zCanvasLoc); //from origin to ref point
 
  stroke(0, 0, 255);
  
  pushMatrix();
  translate(xOffset, yOffset); // redefine the origin
  rotate(angle3);       // rotate in radians
  line(0, 0, zCanvasLoc, 0);  // draw line the seglength
  popMatrix();
  float zLen = 0; // length
  // trace
  text("x " +x , 50, 260);
  text("angle3 " + degrees(angle3) , 50, 275);
  text("xFakMaux " + xFakMaux , 50, 290);
  
   float rotOffsetLength = sqrt(pow(zCanvasLoc, 2) + pow(abs(xFakMaux), 2)) - zCanvasLoc; // sqrt((zCanvasLoc)^2 + (abs(xFakMaux))^2); // amount to adjust other algorhythms
  
   text("rotOffsetLength " + rotOffsetLength , 50, 315); 
   text("angle2Servo0(angle3) " + angle2Servo0(angle3) , 50, 330); 
   text("FakCount " + FakCount , 50, 345); 
   
        port.write("s13000s23000s33140s43000s53000");

  // set servo 0
   port.write(angle2Servo0(angle3));
       //  port.write("s02830");
  // clean up
//  delay(1); // slow to visualize
  if (yFakMaux == FakStart + 2) {
    delay(500);
      FakCount++; // increment count, for osc
       // port.write("s33140s43000s53000");
/*        delay(1500);
     port.write("s02830s13190s22520s34340s41290s53000");  // gesture position - TOO EXTREME
    delay(1500);
         // set servo defaults
    port.write("s33140s43000s50000");
  */  
  }
  stroke(0, 100);
  
}

void segment(float x, float y, float a) {
  pushMatrix();
  translate(x, y); // redefine the origin
  rotate(a);       // rotate in radians
  line(0, 0, segLength, 0);  // draw line the seglength
  popMatrix();
}

